#include <iostream>
#include <fstream>
#include <iomanip>
#include <map>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <sensor_msgs/Imu.h>

// #include "serialport.h"

typedef struct{
    int cnt;
    float gyro_deg[3];
    float accel_mps[3];
}imu_st;


int main(int argc, char** argv){
    
    time_t tt = time(NULL);
    struct tm* t = localtime(&tt);
    std::stringstream filename;
    filename << "/home/nuc/workspace/ai-robot-plus/catkin_ws/data/" 
                << "A15_" << t->tm_year + 1900 << "_"
                << t->tm_mon + 1 << "_" 
                << t->tm_mday << "_" 
                << t->tm_hour << "_" 
                << t->tm_min << "_" 
                << t->tm_sec << ".bin";
    std::ofstream ofs(filename.str(), std::ios::out | std::ios::binary);

    ros::init(argc, argv, "imu_collector");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");


    ros::Duration dt(0);    // insprobe时间与本地时间的间隔
    bool is_dt_initial = false;
    // 订阅insprobe的IMU话题
    ros::Subscriber insprobe_sub = nh.subscribe<sensor_msgs::Imu>("/imu0", 10, [&](const sensor_msgs::Imu::ConstPtr& imu){
        if(!is_dt_initial){
            dt = ros::Time::now() - imu->header.stamp;
            is_dt_initial = true;
        }
    });

    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("/A15/imu", 10);

    serial::Serial ser;
	std::string serial_port;
	int serial_baudrate;
	private_nh.param("serial_port", serial_port, std::string("/dev/ttyACM0"));
	private_nh.param("serial_baudrate", serial_baudrate, 115200);

    // SerialPort serial(serial_port.c_str(), serial_baudrate);
    // if (serial.openPort()) {
    //     std::cout << "Open serial port failed!\n";
    //     return -1;
    // }

	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );
	// 清空串口缓存区
	ser.read(ser.available());
    
    uint8_t state = 0;
    uint8_t data_recv[38] = {0x55, 0x55, 0x55, 0x55};
    uint8_t data_cnt = 4;
    int last_cnt = 0;
    ros::Time timestamp = ros::Time::now();
    ros::Rate loo_rate(100000);
    while(ros::ok()){
        loo_rate.sleep();
        ros::spinOnce();


        if(ser.available())
		{
            uint8_t data = 0;
            ser.read(&data, 1);
            // auto len = serial.readBytes(&data, 1);
            
            if(state == 0 && data == data_recv[state]){
                timestamp = ros::Time::now(); // 记录imu消息到达时间
                state++;
            }
            else if(state == 1 && data == data_recv[state])
                state++;
            else if(state == 2 && data == data_recv[state])
                state++;
            else if(state == 3 && data == data_recv[state])
                state++;
            else if(state == 4){
                data_recv[data_cnt++] = data;
                if(data_cnt >= 38){
                    data_cnt = 4;
                    state = 0;
                    uint8_t checkout = 0;
                    for(int i = 0; i < 37; i++){
                        checkout += data_recv[i];
                    }
                    if(checkout == data_recv[37]){
                        imu_st* imu = (imu_st*)(data_recv + 4);
                        ofs.write((char*)imu, sizeof(imu_st));

                        ROS_INFO("cnt: %d", imu->cnt);

                        // 发布A15惯导数据
                        sensor_msgs::Imu imu_msg;
                        imu_msg.header.frame_id = "A15";
                        imu_msg.header.stamp = timestamp - dt;
                        imu_msg.angular_velocity.x = imu->gyro_deg[0];
                        imu_msg.angular_velocity.y = imu->gyro_deg[1];
                        imu_msg.angular_velocity.z = imu->gyro_deg[2];
                        imu_msg.linear_acceleration.x = imu->accel_mps[0];
                        imu_msg.linear_acceleration.y = imu->accel_mps[1];
                        imu_msg.linear_acceleration.z = imu->accel_mps[2];
                        imu_pub.publish(imu_msg);
                    }
                }
            }
            else state = 0;
        }
    }
    ofs.close();

}